﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;
using Microsoft.WindowsMobile.Samples.Location;
using OpenGLES;
using System.Windows.Forms;
using System.Xml;

namespace PFD
{
    public class SysVarChangeEventArgs<T> : EventArgs
    {
        private T val;
        private string name;

        public SysVarChangeEventArgs(T v, string n)
        {
            val = v;
            name = n;
        }

        public T Value
        {
            get { return val; }
            set { val = value; }
        }

        public string Name
        {
            get { return name; }
            set { name = value; }
        }
    }

    public class SystemVariable<T>  : ISystemVariable<T>
    {
        protected string name;
        protected T val;

        public string Name
        {
            get { return name; }
            set { name = value; }
        }

        public T Value
        {
            get { return val; }
            set 
            {
                if (val.Equals(value))
                    return;
                val = value; 
                NotifyValueChange(); 
            }
        }

        public SystemVariable(string name, T v)
        {
            Name = name;
            val = v;
        }

        public event EventHandler<SysVarChangeEventArgs<T>> OnChange;

        public void NotifyValueChange()
        {
            EventHandler<SysVarChangeEventArgs<T>> temp = OnChange;
            if (OnChange != null)
                OnChange(this, new SysVarChangeEventArgs<T>(val, Name));
        }
    }

    public class GeneralVarsSystem
    {
        public SystemVariable<float> LandscapeOrientation;
        public SystemVariable<float> PortraitOrientation;

        public GeneralVarsSystem()
        {
            //  Default is portrait
            PortraitOrientation = new SystemVariable<float>("PortraitOrientation", 1.0f);
            LandscapeOrientation = new SystemVariable<float>("LandscapeOrientation", 0.0f);
        }

        public void SetOrientation(bool bLandscape)
        {
            if (bLandscape)
            {
                LandscapeOrientation.Value = 1.0f;
                PortraitOrientation.Value = 0.0f;
            }
            else
            {
                LandscapeOrientation.Value = 0.0f;
                PortraitOrientation.Value = 1.0f;
            }
        }

        public void Trigger()
        {
            LandscapeOrientation.NotifyValueChange();
            PortraitOrientation.NotifyValueChange();
        }
    }


    public class GSensorSystem : IDisposable
    {
        public struct HTCGSensorData
        {
            public short TiltX;          // From -1000 to 1000 (about), 0 is flat
            public short TiltY;          // From -1000 to 1000 (about), 0 is flat
            public short TiltZ;          // From -1000 to 1000 (about), 0 = Straight up, -1000 = Flat, 1000 = Upside down
            public short Unknown1;       // Always zero
            public int AngleY;         // From 0 to 359
            public int AngleX;         // From 0 to 359
            public int Unknown2;       // Bit field?
        };

        enum HTCSensor : uint
        {
            GSensor = 1
        }
        [DllImport("HTCSensorSDK")]
        extern static IntPtr HTCSensorOpen(HTCSensor sensor);

        [DllImport("HTCSensorSDK")]
        extern static void HTCSensorClose(IntPtr handle);

        [DllImport("HTCSensorSDK")]
        extern static IntPtr HTCSensorGetDataOutput(IntPtr handle, out HTCGSensorData sensorData);

        public SystemVariable<float> rollAngle;
        public SystemVariable<float> pitchAngle;

        protected float oldRollAngle;
        protected float oldPitchAngle;

        protected float rollOffset;
        protected float pitchOffset;

        bool bFilterActive = false;
        public bool Filter
        {
            get { return bFilterActive; }
            set { bFilterActive = value; }
        }

        private IntPtr myHandle = (IntPtr)null;

        private ScreenOrientation screenOrientation;
        public ScreenOrientation ScreenOrientation
        {
            get { return screenOrientation; }
            set
            {
                if (value != screenOrientation)
                {
                    screenOrientation = value;
                }
            }
        }

        private OperationalMode opMode;
        public OperationalMode OperationalMode
        {
            get { return opMode; }
            set
            {
                if (value == OperationalMode.Simulated)
                {
                    if (myHandle != (IntPtr)null)
                        HTCSensorClose(myHandle);

                    myHandle = (IntPtr)null;
                    opMode = value;
                }
                if (value == OperationalMode.RealWorld)
                {
                    if (myHandle == (IntPtr)null)
                    {
                        try
                        {
                            myHandle = HTCSensorOpen(HTCSensor.GSensor);
                            opMode = value;
                        }
                        catch (Exception)
                        {
                            myHandle = (IntPtr)null;
                            MessageBox.Show("HTC G-Sensor was not detected.\r\nSwitching to simulation mode.", "Initialization problem", MessageBoxButtons.OK, MessageBoxIcon.Exclamation, MessageBoxDefaultButton.Button1);
                            opMode = OperationalMode.Simulated;
                        }
                    }
                }
            }
        }

        public GSensorSystem(OperationalMode om)
        {
            OperationalMode = om;
            screenOrientation = ScreenOrientation.Portrait;
            pitchAngle = new SystemVariable<float>("PitchAngle", 0.0f);
            rollAngle = new SystemVariable<float>("RollAngle", 0.0f);
            oldPitchAngle = 0.0f;
            oldRollAngle = 0.0f;
            pitchOffset = 0.0f;
            rollOffset = 0.0f;
        }

        public void Update(float fDelta)
        {
            if (opMode == OperationalMode.RealWorld)
                UpdateReal(fDelta);
        }

        private void UpdateReal(float fDelta)
        {
            float currentPitch, currentRoll;

            HTCGSensorData d;
            HTCSensorGetDataOutput(myHandle, out d);

            if (screenOrientation == ScreenOrientation.Portrait)
            {
                Vector3f gVect1 = new Vector3f(0.0f, d.TiltY, d.TiltZ);
                Vector3f gVect2 = new Vector3f(d.TiltX, d.TiltY, 0.0f);

                //  if the device is face up on a flat table, the gvector will
                //  be (0, 0, -9.8)
                //  in order to get the pitchAngle we have to divide the z coord
                //  by 9.8 and acos the result to get the angle in RAD.
                currentPitch = (float)Math.Acos(-gVect1.Z / gVect1.Length);

                currentRoll = (float)Math.Asin(gVect2.X / gVect2.Length);
            }
            else
            {
                Vector3f gVect1 = new Vector3f(d.TiltX, 0, d.TiltZ);
                Vector3f gVect2 = new Vector3f(d.TiltX, d.TiltY, 0.0f);
                //  if the device is face up on a flat table, the gvector will
                //  be (0, 0, -9.8)
                //  in order to get the pitchAngle we have to divide the z coord
                //  by 9.8 and acos the result to get the angle in RAD.
                currentPitch = (float)Math.Acos(-gVect1.Z / gVect1.Length);

                currentRoll = (float)Math.Asin(gVect2.Y / gVect2.Length);
            }
            currentPitch = currentPitch / (float)Math.PI * 180.0f - 90.0f;
            currentRoll = currentRoll / (float)Math.PI * 180.0f;
            if (bFilterActive)
            {
                pitchAngle.Value = (oldPitchAngle + currentPitch) / 2 - pitchOffset;
                rollAngle.Value = (oldRollAngle + currentRoll) / 2 - rollOffset;
            }
            else
            {
                pitchAngle.Value = currentPitch - pitchOffset;
                rollAngle.Value = currentRoll - rollOffset;
            }
            oldPitchAngle = currentPitch;
            oldRollAngle = currentRoll;
        }

        public void setOffset()
        {
            pitchOffset = pitchAngle.Value;
            rollOffset = rollAngle.Value;
        }

        public void resetOffset()
        {
            pitchOffset = rollOffset = 0.0f;
        }

        public void Trigger()
        {
            rollAngle.NotifyValueChange();
            pitchAngle.NotifyValueChange();
        }

        #region IDisposable Members

        public void Dispose()
        {
            if (myHandle != null)
            {
                try
                {
                    HTCSensorClose(myHandle);
                }
                catch (Exception)
                {
                }
                myHandle = (IntPtr)null;
            }
        }

        #endregion
    }


    public class CompassSystem : IDisposable
    {
        HTCCompass compass;
        public SystemVariable<float> Heading;

        OperationalMode opMode;
        public OperationalMode OperationalMode
        {
            get
            {
                return opMode;
            }
            set
            {
                if (value == OperationalMode.RealWorld)
                {
                    compass.Reset();
                }
                else
                {
                    compass.Stop();
                }
                opMode = value;
            }
        }

        public CompassSystem(OperationalMode om)
        {
            compass = new HTCCompass();

            Heading = new SystemVariable<float>("Heading", 0.0f);

            OperationalMode = om;
        }


        public void Update(float fDelta)
        {
            if (opMode == OperationalMode.RealWorld)
                UpdateReal(fDelta);
        }

        private void UpdateReal(float fDelta)
        {
            compass.updateData();
            Heading.Value = (float)compass.Angle;
        }

        public void Trigger()
        {
            Heading.NotifyValueChange();
        }

        #region IDisposable Members

        public void Dispose()
        {
            compass.Stop();
        }

        #endregion
    }


    public class GpsSystem :IDisposable
    {
        public SystemVariable<float> course;
        public SystemVariable<float> speed;
        public SystemVariable<float> altitude;

        Gps gps = new Gps();

        private OperationalMode opMode;
        public OperationalMode OperationalMode
        {
            get { return opMode; }
            set
            {
                if (value == OperationalMode.Simulated)
                    gps.Close();
                if (value == OperationalMode.RealWorld)
                    gps.Open();
                opMode = value;
            }
        }

        public GpsSystem(OperationalMode om)
        {
            opMode = om;
            course = new SystemVariable<float>("Heading", 0.0f);
            speed = new SystemVariable<float>("Airspeed", 0.0f);
            altitude = new SystemVariable<float>("Altitude", 0.0f);
        }

        public void Update(float fDelta)
        {
            if (opMode == OperationalMode.RealWorld)
                UpdateReal(fDelta);
        }

        private void UpdateReal(float fDelta)
        {
            if (gps.Opened)
            {
                GpsPosition position;
                position = gps.GetPosition();

                if (position.HeadingValid)
                    course.Value = position.Heading;                    // in DEG
                if (position.SpeedValid)
                    speed.Value = position.Speed;                         // in knots
                if (position.SeaLevelAltitudeValid)
                    altitude.Value = position.SeaLevelAltitude * 3.28084f;   // in meters * 3.28084 = feet
            }
        }

        public void Trigger()
        {
            course.NotifyValueChange();
            speed.NotifyValueChange();
            altitude.NotifyValueChange();
        }

        #region IDisposable Members

        public void Dispose()
        {
            gps.Close();
        }

        #endregion
    }


    public class AutopilotSystem
    {
        public SystemVariable<bool> MasterEngage;
        public SystemVariable<bool> AutoThrottleEngage;
        public SystemVariable<bool> FlightDirectorEngage;

        public SystemVariable<bool> HeadingLock;
        public SystemVariable<float> HeadingValue;

        public SystemVariable<bool> AltitudeLock;
        public SystemVariable<float> AltitudeValue;

        public SystemVariable<bool> AirspeedLock;
        public SystemVariable<float> AirspeedValue;

        public SystemVariable<bool> GlideslopeLock;
        public SystemVariable<bool> Nav1Lock;
        public SystemVariable<bool> ApproachLock;

        public AutopilotSystem()
        {
            MasterEngage = new SystemVariable<bool>("AutopilotMaster", false);
            AutoThrottleEngage = new SystemVariable<bool>("AutopilotAutothrottleEngage", false);
            FlightDirectorEngage = new SystemVariable<bool>("AutopilotFlightdirectorEngage", false);

            HeadingLock = new SystemVariable<bool>("AutopilotHeadingLock", false);
            HeadingValue = new SystemVariable<float>("AutopilotHeadingVal", 0.0f);

            AltitudeLock = new SystemVariable<bool>("AutopilotAltitudeLock", false);
            AltitudeValue = new SystemVariable<float>("AutopilotAltitudeVal", 0.0f);

            AirspeedLock = new SystemVariable<bool>("AutopilotAirspeedLock", false);
            AirspeedValue = new SystemVariable<float>("AutopilotAirspeedVal", 0.0f);

            GlideslopeLock = new SystemVariable<bool>("AutopilotGlideslopeLock", false);
            Nav1Lock = new SystemVariable<bool>("AutopilotNav1Lock", false);
            ApproachLock = new SystemVariable<bool>("AutopilotApproachLock", false);
        }

        public void Trigger()
        {
            MasterEngage.NotifyValueChange();
            AutoThrottleEngage.NotifyValueChange();
            FlightDirectorEngage.NotifyValueChange();
            HeadingLock.NotifyValueChange();
            HeadingValue.NotifyValueChange();
            AltitudeLock.NotifyValueChange();
            AltitudeValue.NotifyValueChange();
            AirspeedLock.NotifyValueChange();
            AirspeedValue.NotifyValueChange();
            GlideslopeLock.NotifyValueChange();
            Nav1Lock.NotifyValueChange();
            ApproachLock.NotifyValueChange();
        }
    }

    public class SystemVariables
    {
        public Dictionary<string, List<ISystemVarConnector<float>>> floatVars = new Dictionary<string, List<ISystemVarConnector<float>>>();
        public Dictionary<string, List<ISystemVarConnector<bool>>> boolVars = new Dictionary<string, List<ISystemVarConnector<bool>>>();
        public Dictionary<string, ISystemVariable<float>> objVarsFloat = new Dictionary<string, ISystemVariable<float>>();
        public Dictionary<string, ISystemVariable<bool>> objVarsBool = new Dictionary<string, ISystemVariable<bool>>();

        public static void ReadParamValue(ref GeometryArray Array, int nElement, XmlNode xmlNode)
        {
            float PostProcOffset = 0.0f;
            string VAR;
            float val;
            float normalize = 1.0f;

            //  Check if value needs to be normalized
            if (xmlNode.Attributes["Normalize"] != null)
                normalize = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["Normalize"]);

            //  Read Postprocessing Offset
            if (xmlNode.Attributes["PostNormOffset"] != null)
                PostProcOffset = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["PostNormOffset"]);

            if (xmlNode.Attributes["VAR"] != null)
            {
                //  register this item with updater
                VAR = xmlNode.Attributes["VAR"].Value;

                //  Check if a list item for the given variable already exists
                if (!MainForm.sysVars.floatVars.ContainsKey(VAR))
                {
                    //  create a new list item of no one exists
                    MainForm.sysVars.floatVars[VAR] = new List<ISystemVarConnector<float>>();
                }
                //  Check what type of parametric value is requested
                if ((xmlNode.Attributes["TYPE"] != null) &&
                    (xmlNode.Attributes["TYPE"].Value.ToLower() == "linearequation"))
                {
                    //  create a linear equation object
                    float offset = 1.0f;
                    float delta = 1.0f;
                    if (xmlNode.Attributes["OFFSET"] != null)
                        offset = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["OFFSET"]);
                    if (xmlNode.Attributes["DELTA"] != null)
                        delta = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["DELTA"]);
                    MainForm.sysVars.floatVars[VAR].Add(new LinearEquationConnector(Array, nElement,
                        offset, delta, normalize, PostProcOffset));
                }
                else
                {
                    MainForm.sysVars.floatVars[VAR].Add(new SystemVarConnector<float>(Array, nElement, PostProcOffset));
                }
            }

            val = Helper.ReadFloatFromXmlNodeInnerText(xmlNode);

            Array[nElement] = (val + PostProcOffset) / normalize;
        }

        public static void ReadParamValue(ref IVertexBuffer Array, VertexElementDimension ved, int nElement, XmlNode xmlNode, float normalize)
        {
            float PostProcOffset = 0.0f;
            string VAR;
            float val;

            long nidx = Array.GetElementBufferIndex(ved, nElement);

            //  Check if value needs to be normalized
            if (xmlNode.Attributes["Normalize"] != null)
                normalize = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["Normalize"]);

            if (xmlNode.Attributes["PostNormOffset"] != null)
                PostProcOffset = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["PostNormOffset"]);

            if (xmlNode.Attributes["VAR"] != null)
            {
                //  register this item with updater
                VAR = xmlNode.Attributes["VAR"].Value;

                //  Check if a list item for the given variable already exists
                if (!MainForm.sysVars.floatVars.ContainsKey(VAR))
                {
                    //  create a new list item of no one exists
                    MainForm.sysVars.floatVars[VAR] = new List<ISystemVarConnector<float>>();
                }
                //  Check what type of parametric value is requested
                if ((xmlNode.Attributes["TYPE"] != null) &&
                    (xmlNode.Attributes["TYPE"].Value.ToLower() == "linearequation"))
                {
                    //  create a linear equation object
                    float offset = 1.0f;
                    float delta = 1.0f;
                    if (xmlNode.Attributes["OFFSET"] != null)
                        offset = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["OFFSET"]);
                    if (xmlNode.Attributes["DELTA"] != null)
                        delta = Helper.ReadFloatFromXmlNodeAttribute(xmlNode.Attributes["DELTA"]);
                    MainForm.sysVars.floatVars[VAR].Add(new LinearEquationConnector(Array, nidx,
                        offset, delta, normalize, PostProcOffset));
                }
                else
                {
                    MainForm.sysVars.floatVars[VAR].Add(new SystemVarConnector<float>(Array, nidx, PostProcOffset));
                }
            }

            val = Helper.ReadFloatFromXmlNodeInnerText(xmlNode);

            Array[nidx] = (val + PostProcOffset) / normalize;
        }
    }

}
